#ifndef CONTROL_MAG_H
#define CONTROL_MAG_H

#include "utils.h"
#include "common/pid_control.h"

#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"

namespace control
{


class Track_mag
{
public:

    Track_mag()  = default;
    ~Track_mag() = default;

	// 轨迹跟踪： 输入轨迹信息，定位信息，车辆状态信息(当前行驶速度)，输出：速度给定和角速度给定
    Track_mag( double offset_y,double max_speed, double &vel_cmd, double cycle_time,int stop_step, PIDControl &pid_control_offsety ,ControlParamType controlParamInfo);
     // 路径生成：直线路径和曲线路径
    double      CalculateCurveLength(int path_index,std::vector<Pose_type> curve_path_points,Pose_type &p1, Pose_type &p2,geometry_msgs::msg::Pose2D &vehicle_center);
    double      getLaneHeading(Pose_type p_start,Pose_type p_end);
    double      OmgCommandPlan_new(double init_angle,double current_angle,double target_angle,double omg_max,double &omg_command,uint8_t stop_obs_flag);
    
    void        CalculateOffset(Pose_type &p1, Pose_type &p2, geometry_msgs::msg::Pose2D &p0,Pose_type &projection_point);
    bool        JudgeInterval(Pose_type &p1, Pose_type &p2, Pose_type &projection);
    double      TravelDistance(std::string path_type,double travel_dis,double offset_y,double current_speed,double cycletime);//add 20210315 路径偏差过大时的停车保护
    ProjectionIntervalType CalculateProjectionInterval();

    std::string PathType();
    double      getOffsetY() const;
    double      getOffsetX() const;
    double      getOffsetHeading() const;
    double      getOffsetSpeed() const;
    double      getVelCmd();
    double      getWCmd();
    double      getW_offsety();
    double      getW_offsetheading();
    double      getTargetDis();
    double      getDecNew() const;
    double      getTravelDistance() const;
    double      getCosTheta() const;
    bool        TrajectoryTracking_complete() const;
    int         getStopStep();
    void        Clear();



    double      VelocityPlan(std::string path_type,double &max_speed,double &vel_cmd,int &stop_step, int &lift_flag);
    double      PIDController(double offset_y,ControlParamType controlParamInfo,double dt,PIDControl &pid_control_offsety);

    double      getLeftSpeed();
    double      getRightSpeed();    
private:



    uint8_t stop_obs_flag_;
    double  max_speed_;
    double  target_speed_;
    double  vehicle_speed_;


    double  w_offsety_;
    int     stop_step_;
    int     turn_direction_;


    PIDControl       pid_control_offsety_;
    double offset_y_;
    ControlParamType controlParamInfo_;
    double  vel_cmd_;
    double  w_cmd_;
    double  left_speed_;
    double  right_speed_;
    double  cycle_time_;
    double  acc_;
};


}

#endif
